#include "MicroStrain_3DM_GX2.h"

//Constructor and destructor
MicroStrain_3DM_GX2::MicroStrain_3DM_GX2(): measurement_nr(0), continuous_mode_set(0) {;}
MicroStrain_3DM_GX2::~MicroStrain_3DM_GX2() {;}

//Open IMU object
int MicroStrain_3DM_GX2::open() {
	//Open serial port
	serial_port.open_port("/dev/ttyUSB0", 115200, NO_PARITY, 8, 1);
	//Dynamically allocate read buffer
	read_buffer=new unsigned char[43];
	return 1; }

//Close IMU object
int MicroStrain_3DM_GX2::close() {
	//Check if IMU is set to silent
	if(continuous_mode_set){
		stop();}
	//Close serial port
	serial_port.close();
	return 1;}

//Check what data rate the IMU is set to. -- Andblo 100406 UNTESTED! DO NOT USE!!!!
/*void MicroStrain_3DM_GX2::checkDataRate() {
	if(!continuous_mode_set){
		unsigned char byte0 = 0xe5;         //Read EEPROM-command
		unsigned char byte1 = 0x00;         //empty
		unsigned char byte2 = 0xfc;         //Address to data rate info in EEPROM
		unsigned char byte3 = 0xa2;         //Address to data rate info in EEPROM
		unsigned char command[] = {byte0,byte1,byte2,byte3}; //Total command

		write_buffer = command;
		//Clear Serial receiving buffer
		serial_port.flush();

		//Send Command
		if(!serial_port.write_binary(write_buffer,4)){
			cout << "Write operation to Accelerometer failed" << endl;
			//Command could not be sent
		} else if(!serial_port.read_binary(read_buffer, 5)){ //Read response
			cout << "Read data rate failed. No response from IMU" << endl;
			//IMU did not reply with datarate
		} else { //read reply
			unsigned char header = read_buffer[0];
			unsigned char cByte[] = {read_buffer[2],read_buffer[1]};
			unsigned char read_checksum[] = {read_buffer[4],read_buffer[3]};

			//Calculate checksum
			unsigned short calc_checksum = 0;
			calculate_checksum(read_buffer, 4, &calc_checksum);

			if (!((calc_checksum==*reinterpret_cast<unsigned short *>(read_checksum)) &&
				     (0xe5         ==header))) {
				cout << "Incorrect reply to check data rate from IMU" << endl;
			} else {

			}

		}

	} else {
		cout << "Cannot check data rate while in continuous mode" << endl;
	}

}*/

//Set the IMU to continuous output
int MicroStrain_3DM_GX2::run() {

	/*//read the rate
	unsigned short address=0xfca2;
	unsigned short read_word;
	read_word_from_EEPROM(&address,&read_word);
*/

	//Is continuous mode set?
	if(!continuous_mode_set){
		//Set continuous mode
		unsigned char byte0 = 0xc4;         //Command Byte/header
		unsigned char byte1 = 0xc1;         //Confirms user intent
		unsigned char byte2 = 0x29;         //Confirms user intent
		//unsigned char byte3 = 0xc2;         //Continuous Command Byte (Acceleration and Angular rate)
		unsigned char byte3 = 0xcb;         //Continuous Command Byte (Acceleration and Angular rate+magnetometer)
		unsigned char command[] = {byte0,byte1,byte2,byte3}; //Total command

		write_buffer = command;
		//Clear Serial receiving buffer
		serial_port.flush();
		//Send Command
		if(!serial_port.write_binary(write_buffer,4)){
			cout << "Write operation to Accelerometer failed" << endl;
			//Command could not be sent
			return 0;}
		//Read response
		if(!serial_port.read_binary(read_buffer, 8)){
			cout << "Set continuous mode failed. No response from IMU" << endl;
			//IMU could not be set to continuous mode
			return 0;}
		//Response
		unsigned char header = read_buffer[0];
		unsigned char cByte = read_buffer[1];
		unsigned char timer[] = {read_buffer[5],read_buffer[4],read_buffer[3],read_buffer[2]};
		unsigned char read_checksum[] = {read_buffer[7],read_buffer[6]};
		//Calculate checksum
		unsigned short calc_checksum = 0;
		calculate_checksum(read_buffer, 6, &calc_checksum);
		//Check acquired checksum, the header, and the command byte
		while (!((calc_checksum==*reinterpret_cast<unsigned short *>(read_checksum)) &&
			     (0xc4         ==header) &&
				 (0xcb         ==cByte))){
			printf("Incorrect reply sending continuous mode request\n");
			//Flush serial buffer
			serial_port.flush();
			//Send stop continuous command again
			stop();
			//Since IMU seems to be hard to get through to
			//we send the command several times.
			for (int i=0;i<5;i++){
				serial_port.write_binary(write_buffer,4);}
			//Read response
			serial_port.read_binary(read_buffer,8);
			//Response
			header = read_buffer[0];
			cByte = read_buffer[1];
			timer[0] = read_buffer[5];
			timer[1] = read_buffer[4];
			timer[2] = read_buffer[3];
			timer[3] = read_buffer[2];
			read_checksum[0] = read_buffer[7];
			read_checksum[1] = read_buffer[6];
			//Compute checksum
			calc_checksum = 0;
			calculate_checksum(read_buffer, 6, &calc_checksum);
		}
		//Get rid of possible garbage in buffer
		serial_port.flush();
		//Set contiuous mode successfull
		printf("----Continuous mode on IMU----\n");
		continuous_mode_set=1;
		return 1;}
	printf("----Continuous mode on IMU already set----\n");
	return 1;}

//Set the IMU to silent
int MicroStrain_3DM_GX2::stop() {
	//Is continuous mode set?
	if (continuous_mode_set){
		//Set IMU to silent
		unsigned char byte0 = 0xc4;         //Command Byte
		unsigned char byte1 = 0xc1;         //Confirms user intent
		unsigned char byte2 = 0x29;         //Confirms user intent
		unsigned char byte3 = 0x00;         //Contiuous Command Byte (00=stop continuous mode)
		unsigned char command[] = {(char) byte0, (char) byte1, (char) byte2, (char) byte3}; //Total command
		write_buffer = command;
		//Flush Serial buffer
		if(!serial_port.flush()){
			cout << "Serial port buffer could not be cleared" << endl;
			//Buffer could not be flushed
			return 0;}
		//Send command
		if(!serial_port.write_binary(write_buffer, 4)){
			cout << "Write operation to Accelerometer failed" << endl;
			//IMU could not be set to silent
			return 0;}
		//Read response
		if(!serial_port.read_binary(read_buffer, 8)){
			cout << "Read operation from Accelerometer failed" << endl;
			//Could not read data from IMU
			return 0;}
		//Response
		unsigned char header = read_buffer[0];
		unsigned char cByte = read_buffer[1];
		unsigned char timer[] = {read_buffer[5],read_buffer[4],read_buffer[3],read_buffer[2]};
		unsigned char checksum[] = {read_buffer[7],read_buffer[6]};
		//Check the integrity of the data
		//Compute checksum
		unsigned short checksum_calc = 0;
		calculate_checksum(read_buffer, 6, &checksum_calc);
		//Check aquired checksum, the header, and the command byte
		while (!((checksum_calc==*reinterpret_cast<unsigned short *>(checksum)) &&
				 (0xc4         ==header) &&
				 (0x00         ==cByte)    )){
			//Flush serial buffer
			serial_port.flush();
			//Send stop command again
			//Since IMU seems to be hard to get through to
			//we send the command several times.
			for (int i=0;i<5;i++){
				//serial_port.write_binary(write_buffer,4);
				if(!serial_port.write_binary(write_buffer, 4)){
					cout << "Write operation to Accelerometer failedX" << endl;
					//IMU could not be set to silent
					return 0;}
				}
			//Read response
			serial_port.read_binary(read_buffer,8);
			//Response
			header = read_buffer[0];
			cByte = read_buffer[1];
			timer[0] = read_buffer[5];
			timer[1] = read_buffer[4];
			timer[2] = read_buffer[3];
			timer[3] = read_buffer[2];
			checksum[0] = read_buffer[7];
			checksum[1] = read_buffer[6];
			//Compute checksum
			checksum_calc = 0;
			calculate_checksum(read_buffer, 6, &checksum_calc);}
		//Get rid of possible garbage in buffer
		serial_port.flush();
		//stopIMU succesfull
		printf("----Continuous mode off IMU----\n");
		continuous_mode_set=0;
		return 1;}
	printf("----Continuous mode on IMU not set----\n");
	return 1;}

Inertial_measurement* MicroStrain_3DM_GX2::read() {
	//If continuous mode is not set, set it
	if (!continuous_mode_set) {
		run();
	}
	//Read from buffer
	if (!serial_port.read_binary(read_buffer, 43)) {
		cout << "Read operation from Accelerometer failed" << endl;
		//Could not read data from IMU
		return 0;
	}
	//Check the integrity of the data
	//Read checksum
	char read_checksum[] = { read_buffer[42], read_buffer[41] };
	//Compute checksum
	unsigned short calc_checksum = 0;
	calculate_checksum(read_buffer, 41, &calc_checksum);

	//Compare to acquired checksum
	while (!(calc_checksum	== *reinterpret_cast<unsigned short *> (read_checksum))) {

		printf("Incorrect checksum, IMU\n");
		//if not correct we search for a new header in the buffer
		//header=0xc2
		//Search index, 0-index if probably header so we start on 1 to find new header
		int read_new_buffer=1;
		for (int i=1;i<43;i++){
			//If we find header we make a new read operation from there
			if (0xcb==*(read_buffer+i)){
				//Move array components
				for (int j=0;j+i<43;j++){
					read_buffer[j]=read_buffer[i+j];}
				//Read "i" new data to get a complete set of 31 bytes
				serial_port.read_binary(&read_buffer[43-i],i);
				//Timestamp for data
				//time_stamp = Time_stamper::get_time_stamp();
				read_new_buffer=0;
				break;
			}
		}
		//If no new header were found read complete new buffer
		if (read_new_buffer){
			serial_port.read_binary(read_buffer,43);
			//Timestamp for data
			//time_stamp = Time_stamper::get_time_stamp();
			}
		//Read checksum
		read_checksum[0] = read_buffer[42];
		read_checksum[1] = read_buffer[41];
		//Compute checksum
		calc_checksum = 0;
		calculate_checksum(read_buffer, 41, &calc_checksum);
	}

	//Dynamic allocation of space for measurement
	Inertial_measurement *inertial_measurement = new Inertial_measurement;

	//Change byteorder
	unsigned char header=read_buffer[0];
	char xacc[]={read_buffer[4],read_buffer[3],read_buffer[2],read_buffer[1]};
	char yacc[]={read_buffer[8],read_buffer[7],read_buffer[6],read_buffer[5]};
	char zacc[]={read_buffer[12],read_buffer[11],read_buffer[10],read_buffer[9]};
	char xar[]={read_buffer[16],read_buffer[15],read_buffer[14],read_buffer[13]};
	char yar[]={read_buffer[20],read_buffer[19],read_buffer[18],read_buffer[17]};
	char zar[]={read_buffer[24],read_buffer[23],read_buffer[22],read_buffer[21]};
	char xm[]={read_buffer[28],read_buffer[27],read_buffer[26],read_buffer[25]};
	char ym[]={read_buffer[32],read_buffer[31],read_buffer[30],read_buffer[29]};
	char zm[]={read_buffer[36],read_buffer[35],read_buffer[34],read_buffer[33]};
	char timer[]= {read_buffer[40],read_buffer[39],read_buffer[38],read_buffer[37]};
	char checksum[]= {read_buffer[42],read_buffer[41]};

	//Measurement nr
	measurement_nr++;
	inertial_measurement->nr = measurement_nr;

	//Cast and add to inertial_measurement
	inertial_measurement->header=header;  //only for debugging
	inertial_measurement->a1=*reinterpret_cast<float *>(xacc);
	inertial_measurement->a2=*reinterpret_cast<float *>(yacc);
	inertial_measurement->a3=*reinterpret_cast<float *>(zacc);
	inertial_measurement->ar1=*reinterpret_cast<float *>(xar);
	inertial_measurement->ar2=*reinterpret_cast<float *>(yar);
	inertial_measurement->ar3=*reinterpret_cast<float *>(zar);
	inertial_measurement->m1=*reinterpret_cast<float *>(xm);
	inertial_measurement->m2=*reinterpret_cast<float *>(ym);
	inertial_measurement->m3=*reinterpret_cast<float *>(zm);
	inertial_measurement->source_timer=*reinterpret_cast<unsigned int *>(timer);
	inertial_measurement->checksum=*reinterpret_cast<unsigned short int *>(checksum); //Only for debugging


	//Return pointer to dynamically allocated measurement object
	return inertial_measurement;
}


int MicroStrain_3DM_GX2::read_word_from_EEPROM(unsigned short* address,unsigned short* word){
	//Command
	unsigned char byte0 = 0xe5;         //Command byte
	unsigned char byte1 = 0x00;         //Empty byte
	unsigned char byte2 = reinterpret_cast<unsigned char *>(address)[1];         //Address byte 1
	unsigned char byte3 = reinterpret_cast<unsigned char *>(address)[0];         //Address byte 2
	unsigned char command[] = {byte0,byte1,byte2,byte3}; //Total command
	write_buffer = command;
	//Clear Serial receiving buffer
	serial_port.flush();
	//Send Command
	if(!serial_port.write_binary(write_buffer,4)){
		cout << "Read operation from IMU failed" << endl;
		//Command could not be sent
		return false;}
	//Read response
	if(!serial_port.read_binary(read_buffer, 5)){
		cout << "Read word from EEPROM failed. No response from IMU" << endl;
		//IMU could not be set to continuous mode
		return false;}
	//Response
	unsigned char header = read_buffer[0];
	unsigned char read_word[] = {read_buffer[2],read_buffer[1]};
	unsigned char read_checksum[] = {read_buffer[4],read_buffer[3]};
	//Calculate checksum
	unsigned short calc_checksum = 0;
	calculate_checksum(read_buffer, 3, &calc_checksum);
	//Check aquired checksum, the header, and the command byte
	if (!((calc_checksum==*reinterpret_cast<unsigned short *>(read_checksum)) &&
		     (0xe5         ==header))){
	/*	printf("Ch-sum-calc:       %#x \n",calc_checksum);
		//printf("Ch-sum-calc:  %#x \n",*reinterpret_cast<unsigned short *>(&checksum));
		printf("Ch-sum:  %#x \n",read_checksum);
		printf("Header:       %#x \n", header);
//		printf("Command Byte: %#x \n", cByte);
		//Flush serial buffer
		serial_port.flush();
		//Send stop continuous command again
		//Since IMU seems to be hard to get through to
		//we send the command several times.
//		for (int i=0;i<5;i++){
//			serial_port.write_binary(write_buffer,4);
//		}
		//Read response
//		serial_port.read_binary(read_buffer,5);
		//Response
		header = read_buffer[0];
		read_word[0] = read_buffer[2];
		read_word[1] = read_buffer[1];
		read_checksum[0] = read_buffer[4];
		read_checksum[1] = read_buffer[3];
		//Compute checksum
		calc_checksum = 0;
		calculate_checksum(read_buffer, 6, &calc_checksum);*/
		cout << "Failed to read response - checksum mismatch" << endl;
		return(0);
	}
	//Get rid of possible garbage in buffer
	serial_port.flush();
	//Output the respons of the IMU
	//cout << "-----Continuous mode on IMU-----" << endl;
	//cout << "Clock: " << clock() << endl;
	/*cout << "----Response----" << endl;
	//printf("Header:       %#x \n", header);
	//printf("Command Byte: %#x \n", cByte);
	printf("Timer:        %u \n", *reinterpret_cast<unsigned int *>(timer));
	printf("Checksum:     %#x \n", *reinterpret_cast<unsigned short *>(read_checksum));
	cout << "----------------" << endl;
	cout << "---------------------------------" << endl << endl;

	//Set contiuous mode successfull
*/
	printf("Read from EEPROM word=%i, address=%0x\n",*reinterpret_cast<unsigned short *>(read_word),*address);
	*word=*reinterpret_cast<unsigned short *>(read_word);
	return true;}

int MicroStrain_3DM_GX2::calculate_checksum(unsigned char *data, int length, unsigned short *checksum) {
	unsigned short sum=0;
    while (length-- > 0)
    {
        sum += *(data++);
    }
    *checksum=sum;

	return 1; }
